#include <chrono>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <sys/time.h> 
#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>

#include "camera_node.hpp"
#include "act_D455.h"
#include "yolov8.hpp"
#include "pcl_process.hpp"
#include "trajectory_prediction.hpp"
#include "mind_vision.hpp"
#include "depth_calculating.hpp"


// 用于指示是否应该退出程序的全局标志
atomic<bool> should_exit(false);

void sigint_handler(int signum) {
  cout << "Received SIGINT signal (" << signum << ")" << endl;
  should_exit = true;
}

int main(int argc, char* argv[]) {
    signal(SIGINT, sigint_handler);// 注册 SIGINT 信号处理程序
	
	cudaSetDevice(0);
    const std::string engine_file_path{"./src/binocular_camera/modejson/2024_10_20.engine"};
    auto yolov8 = new YOLOv8(engine_file_path);// yolov8类初始化引擎路径
    yolov8->make_pipe(true);// 热身，对模型内存调用等进行初始化

	// ActD455 D455;
	// D455.Init();

	PclProcess pcl_process("roi_cloud");

	cv::TickMeter tk;
	cv::TickMeter tk2;
	cv::TickMeter tk3;
	timeval time;
	MindVision mvcamera;
	DepthCal depth_cal;

	Mat frame, left_frame, right_frame;
	cv::Rect rect(0, 0, 640, 480);
	// int disp_flag = 0;
	CameraHandle handle_cam = mvcamera.get_hcamera();
	thread timer_threads(&MindVision::print_fps_callback, &mvcamera, &handle_cam);
	timer_threads.detach();

	while (!should_exit) {

		tk.stop();
		// cout << "------------------------每帧总时间:" << tk.getTimeMilli() << endl;
		tk.reset();
		tk.start();
		gettimeofday(&time, NULL);

		tk2.reset();
		tk2.start();
		mvcamera.frame_update(); // 8-9ms
		tk2.stop();
		// cout << "------------------------图像采集时间:" << tk2.getTimeMilli() << endl;
		
		tk2.reset();
		tk2.start();
		depth_cal.stereo_correction(mvcamera.get_frame_l(), mvcamera.get_frame_r()); // 5-6ms
		tk2.stop();
		// cout << "------------------------立体矫正时间:" << tk2.getTimeMilli() << endl;

		// namedWindow("left_img", WINDOW_NORMAL);
		// imshow("left_img", mvcamera.get_frame_l());
		// namedWindow("right_img", WINDOW_NORMAL);
		// imshow("right_img", mvcamera.get_frame_r());
		// int key = waitKey(1);
		// if(key == 'p'){
		// 	cout << cv::imwrite(format("./data/frame/camL/%d_%d.bmp", time.tv_sec, time.tv_usec), mvcamera.get_frame_l()) << endl; //保存图片
		// 	cout << cv::imwrite(format("./data/frame/camR/%d_%d.bmp", time.tv_sec, time.tv_usec), mvcamera.get_frame_r()) << endl; //保存图片
		// }
		tk2.reset();
		tk2.start();
		yolov8->detect(depth_cal.get_rectifyImageL()); // 8-9ms
		tk2.stop();
		// cout << "------------------------yolo检测时间:" << tk2.getTimeMilli() << endl;
		
		if(yolov8->resultFlag){
			tk2.reset();
			tk2.start();
			depth_cal.stereo_match(depth_cal.get_rectifyImageL(), depth_cal.get_rectifyImageR(), yolov8->objs.at(0).rect); // 1-2ms
			tk2.stop();
			// cout << "------------------------深度计算时间:" << tk2.getTimeMilli() << endl;

		}

		pcl::copyPointCloud(*depth_cal.get_point_cloud(), *pcl_process.roi_cloud);
		// cout << pcl_process.roi_cloud->size() << endl;
		
		tk2.reset();
		tk2.start();
		namedWindow("result", WINDOW_NORMAL);
		cv::imshow("result", yolov8->res); // 4-5ms
		pcl_process.Pcl_show(); // 2-3ms
		int key = waitKey(1);
		tk2.stop();
		// cout << "------------------------原图及点云显示时间:" << tk2.getTimeMilli() << endl;

		mvcamera.fps_count++;

	}

	cv::destroyAllWindows();
}
